within ThreeD_MBS_Dynamics.Examples.Snakeboard;

model Test_Kuleshov
  parameter SI.Mass m=5;
  parameter SI.Length L=2.5;
  parameter SI.MomentOfInertia J=0.1442;
  parameter SI.MomentOfInertia Jr=0.125;
  parameter SI.MomentOfInertia Jp=0.1275;
  parameter SI.Angle a_psi=0.7;
  parameter SI.Angle b_psi=0;
  parameter SI.AngularVelocity w0_psi=1;
  parameter SI.Angle a_phi=0.3;
  parameter SI.Angle b_phi=0.01;
  parameter SI.AngularVelocity w0_phi=1;
  SI.Angle psi;
  SI.Angle phi;
  SI.Angle theta;
  SI.AngularVelocity w_theta;
  SI.AngularVelocity w_phi;
  SI.AngularVelocity w_psi;
  SI.Torque u_psi;
  SI.Torque u_f;
  SI.Torque u_b;
  SI.Position x;
  SI.Position y;
  SI.Position z;
  SI.Velocity vx;
  SI.Velocity vy;
  SI.Velocity vz;
  SI.Velocity v;
  SI.Acceleration ax;
  SI.Acceleration ay;
  SI.Energy K;
equation
  z = -y;
  der(psi) = w_psi;
  der(phi) = w_phi;
  der(theta) = w_theta;
  der(x) = vx;
  vx = L*der(theta)*cos(theta)/tan(phi);
  der(y) = vy;
  vy = L*der(theta)*sin(theta)/tan(phi);
  vz = -vy;
  der(vx) = ax;
  der(vy) = ay;
  der(w_theta) - (m*L^2)*der(phi)/tan(phi)*w_theta/((J + Jr + 2*Jp)*(sin(phi))
    ^2 + m*L^2*(cos(phi))^2) + Jr*der(w_psi)*(sin(phi))^2/((J + Jr + 2*Jp)*(
    sin(phi))^2 + m*L^2*(cos(phi))^2) = 0;
  Jr*(der(w_psi) + der(w_theta)) = u_psi;
  Jp*(der(w_phi) + der(w_theta)) = u_f;
  Jp*(der(w_theta) - der(w_phi)) = u_b;
  psi = a_psi*sin(w0_psi*time + b_psi);
  phi = a_phi*sin(w0_phi*time + b_phi);
  v = sqrt(vx^2 + vy^2);
  K = 0.5*m*v*v + 0.5*Jp*(w_theta + w_phi)^2 + 0.5*Jp*(w_theta - w_phi)^2 +
    0.5*J*(w_theta)^2 + 0.5*Jr*(w_psi + w_theta)^2;
  annotation (
    Diagram,
    experiment(
      StopTime=25,
      NumberOfIntervals=8000,
      Tolerance=1e-012),
    experimentSetupOutput,
    DymolaStoredErrors);
end Test_Kuleshov;
